#ifndef CONTROLLER_INTERFACE_H
#define CONTROLLER_INTERFACE_H

#ifdef __cplusplus
extern "C" {
#endif
#include <lcmtypes/quad_lcmtypes.h>
#include <quad_lcmtypes/lcm_channel_names.h>
#include <lcm/lcm.h>

typedef struct {
  double default_height;
  double default_yaw;
  double default_xy_vel;
  double default_z_vel;
  double default_yaw_vel;

  double default_takeoff_height;
  double default_takeoff_vel;
  double default_land_vel;

  int enforceGoalReached;
} waypoint_params_t;

/*
 * carmen3d_controller_sanity_check_and_publish:
 * this should always be called to send out waypoints for the quad
 */
static inline int carmen3d_controller_sanity_check_and_publish(
    lcm_t *lc, const char *channel, const quad_waypoint_t *p) {
  if (p->nonce <= 0) {
    fprintf(stderr, "ERROR: waypoint nonce of %d is invalid\n", p->nonce);
    return 0;
  }
  if (p->utime <= 0) {
    fprintf(stderr, "ERROR: waypoint time %jd is <=0\n", p->utime);
    return 0;
  }
  if (p->waypt_type <= 0 || p->waypt_type >= QUAD_WAYPOINT_T_TYPE_MAX_VALID) {
    fprintf(stderr, "ERROR: waypoint type of %d is invalid\n", p->waypt_type);
    return 0;
  }
  if (p->sender <= 0 || p->sender >= QUAD_WAYPOINT_T_SENDER_MAX_VALID) {
    fprintf(stderr, "ERROR: waypoint sender of %d is invalid\n", p->sender);
    return 0;
  }

  //TODO: decide and or impliment checks on x,y,z,yaw,pitch,roll

  quad_waypoint_t_publish(lc, channel, p);

  return 1;
}

/*
 * carmen3d_controller_update_waypoints:
 * this function is used to process a received waypoint, and does sanity checks, as well as filling out defaults, etc...
 * waypt_params       - contains the default waypoint parameters and whatnot
 * new_waypt          - contains the new waypoint that was just received
 * start_point        - contains the current "trajectory" start point, this may be updated
 * current_set_point  - contains the current set_point for the vehicle
 * goal               - contains the current goal point, this may be updated
 * current_pose       - contains the current POSE of the vehicle in global coordinates
 * goalReached        - flag which specifies whether the last goal had been reached
 * current_utime       - current time
 *
 * returns 1 if the waypoint was accepted or 0 if it was ignored
 */
int carmen3d_controller_update_waypoints(waypoint_params_t * waypt_params,
                                         lcm_t * lcm,
                                         const quad_waypoint_t *new_waypt,
                                         quad_waypoint_t * start_point,
                                         quad_waypoint_t *current_set_point,
                                         quad_waypoint_t *goal,
                                         quad_waypoint_t * current_pose,
                                         int goalReached,
                                         int64_t current_utime);

/*
 * carmen3d_controller_set_noop:
 * reset a controller message so that controller would do anything...
 * it should be "safe" to send it, but the timestamp, nonce, and sender should all be filled in
 */
static inline void carmen3d_controller_set_noop(quad_waypoint_t *msg) {
  //default values
  msg->xyzt[0] = QUAD_WAYPOINT_T_USE_PREVIOUS;
  msg->xyzt[1] = QUAD_WAYPOINT_T_USE_PREVIOUS;
  msg->xyzt[2] = QUAD_WAYPOINT_T_USE_PREVIOUS;
  msg->xyzt[3] = QUAD_WAYPOINT_T_USE_PREVIOUS;
  msg->xyzt_dot[0] = QUAD_WAYPOINT_T_USE_DEFAULT;
  msg->xyzt_dot[1] = QUAD_WAYPOINT_T_USE_DEFAULT;
  msg->xyzt_dot[2] = QUAD_WAYPOINT_T_USE_DEFAULT;
  msg->xyzt_dot[3] = QUAD_WAYPOINT_T_USE_DEFAULT;
  msg->nonce = -1;
  msg->utime = -1;
  msg->waypt_type = -1;
  msg->sender = QUAD_WAYPOINT_T_SENDER_YOUR_MOM;
}

/*
 * carmen3d_controller_publish_stop_waypoint:
 * sends out a STOP waypoint, which just stops the quad where it is
 */
static inline void carmen3d_controller_publish_stop_waypoint(lcm_t * lcm,
                                                             int64_t utime) {
  quad_waypoint_t wmsg;
  memset(&wmsg, 0, sizeof(wmsg));  //set everything to 0
  //set to previous/default
  wmsg.xyzt[0] = QUAD_WAYPOINT_T_USE_PREVIOUS;
  wmsg.xyzt[1] = QUAD_WAYPOINT_T_USE_PREVIOUS;
  wmsg.xyzt[2] = QUAD_WAYPOINT_T_USE_PREVIOUS;
  wmsg.xyzt[3] = QUAD_WAYPOINT_T_USE_PREVIOUS;
  wmsg.xyzt_dot[0] = QUAD_WAYPOINT_T_USE_DEFAULT;
  wmsg.xyzt_dot[1] = QUAD_WAYPOINT_T_USE_DEFAULT;
  wmsg.xyzt_dot[2] = QUAD_WAYPOINT_T_USE_DEFAULT;
  wmsg.xyzt_dot[3] = QUAD_WAYPOINT_T_USE_DEFAULT;
  wmsg.nonce = random();
  wmsg.utime = utime;

  wmsg.waypt_type = QUAD_WAYPOINT_T_TYPE_STOP;
  quad_waypoint_t_publish(lcm, WAYPOINT_COMMAND_CHANNEL, &wmsg);
  quad_waypoint_t_publish(lcm, WAYPOINT_PLANNER_GOAL_CHANNEL, &wmsg);

}
/*
 * carmen3d_controller_publish_user_stop_waypoint:
 * sends out a USER_STOP waypoint, which just stops the quad where it is
 */
static inline void carmen3d_controller_publish_user_stop_waypoint(
    lcm_t * lcm, int64_t utime) {
  quad_waypoint_t wmsg;
  memset(&wmsg, 0, sizeof(wmsg));    //set everything to 0
  //set to previous/default
  wmsg.xyzt[0] = QUAD_WAYPOINT_T_USE_PREVIOUS;
  wmsg.xyzt[1] = QUAD_WAYPOINT_T_USE_PREVIOUS;
  wmsg.xyzt[2] = QUAD_WAYPOINT_T_USE_PREVIOUS;
  wmsg.xyzt[3] = QUAD_WAYPOINT_T_USE_PREVIOUS;
  wmsg.xyzt_dot[0] = QUAD_WAYPOINT_T_USE_DEFAULT;
  wmsg.xyzt_dot[1] = QUAD_WAYPOINT_T_USE_DEFAULT;
  wmsg.xyzt_dot[2] = QUAD_WAYPOINT_T_USE_DEFAULT;
  wmsg.xyzt_dot[3] = QUAD_WAYPOINT_T_USE_DEFAULT;
  wmsg.nonce = random();
  wmsg.utime = utime;

  wmsg.waypt_type = QUAD_WAYPOINT_T_TYPE_USER_STOP;
  quad_waypoint_t_publish(lcm, WAYPOINT_COMMAND_CHANNEL, &wmsg);
  quad_waypoint_t_publish(lcm, WAYPOINT_PLANNER_GOAL_CHANNEL, &wmsg);

}

/*
 * carmen3d_controller_publish_land_waypoint:
 * sends out a LAND waypoint, quad will land right where it is
 */
static inline void carmen3d_controller_publish_land_waypoint(lcm_t * lcm,
                                                             int64_t utime) {
  quad_waypoint_t wmsg;
  memset(&wmsg, 0, sizeof(wmsg));    //set everything to 0
  //set to previous/default
  wmsg.xyzt[0] = QUAD_WAYPOINT_T_USE_PREVIOUS;
  wmsg.xyzt[1] = QUAD_WAYPOINT_T_USE_PREVIOUS;
  wmsg.xyzt[2] = QUAD_WAYPOINT_T_USE_PREVIOUS;
  wmsg.xyzt[3] = QUAD_WAYPOINT_T_USE_PREVIOUS;
  wmsg.xyzt_dot[0] = QUAD_WAYPOINT_T_USE_DEFAULT;
  wmsg.xyzt_dot[1] = QUAD_WAYPOINT_T_USE_DEFAULT;
  wmsg.xyzt_dot[2] = QUAD_WAYPOINT_T_USE_DEFAULT;
  wmsg.xyzt_dot[3] = QUAD_WAYPOINT_T_USE_DEFAULT;
  wmsg.nonce = random();
  wmsg.utime = utime;

  wmsg.waypt_type = QUAD_WAYPOINT_T_TYPE_LAND;
  quad_waypoint_t_publish(lcm, WAYPOINT_COMMAND_CHANNEL, &wmsg);

}
/*
 * carmen3d_controller_publish_takeoff_waypoint:
 * sends out a TAKEOFF waypoint, quad will takeoff right where it is
 */
static inline void carmen3d_controller_publish_takeoff_waypoint(lcm_t * lcm,
                                                                int64_t utime) {
  quad_waypoint_t wmsg;
  memset(&wmsg, 0, sizeof(wmsg));    //set everything to 0
  //set to previous/default
  wmsg.xyzt[0] = QUAD_WAYPOINT_T_USE_PREVIOUS;
  wmsg.xyzt[1] = QUAD_WAYPOINT_T_USE_PREVIOUS;
  wmsg.xyzt[2] = QUAD_WAYPOINT_T_USE_PREVIOUS;
  wmsg.xyzt[3] = QUAD_WAYPOINT_T_USE_PREVIOUS;
  wmsg.xyzt_dot[0] = QUAD_WAYPOINT_T_USE_DEFAULT;
  wmsg.xyzt_dot[1] = QUAD_WAYPOINT_T_USE_DEFAULT;
  wmsg.xyzt_dot[2] = QUAD_WAYPOINT_T_USE_DEFAULT;
  wmsg.xyzt_dot[3] = QUAD_WAYPOINT_T_USE_DEFAULT;
  wmsg.nonce = random();
  wmsg.utime = utime;

  wmsg.waypt_type = QUAD_WAYPOINT_T_TYPE_TAKEOFF;
  quad_waypoint_t_publish(lcm, WAYPOINT_COMMAND_CHANNEL, &wmsg);
}
/*
 * carmen3d_controller_publish_relative_waypoint:
 * publishes a waypoint that instructs the quad to move relative to where it is in the GLOBAL frame
 */

static inline void carmen3d_controller_publish_relative_waypoint(
    lcm_t * lcm, double x, double y, double z, double yaw, int64_t utime) {
  quad_waypoint_t wmsg;
  memset(&wmsg, 0, sizeof(wmsg));    //set everything to 0
  //  by previous/default unless otherwise instructed :-)
  wmsg.xyzt[0] = QUAD_WAYPOINT_T_USE_PREVIOUS;
  wmsg.xyzt[1] = QUAD_WAYPOINT_T_USE_PREVIOUS;
  wmsg.xyzt[2] = QUAD_WAYPOINT_T_USE_PREVIOUS;
  wmsg.xyzt[3] = QUAD_WAYPOINT_T_USE_PREVIOUS;
  wmsg.xyzt_dot[0] = QUAD_WAYPOINT_T_USE_DEFAULT;
  wmsg.xyzt_dot[1] = QUAD_WAYPOINT_T_USE_DEFAULT;
  wmsg.xyzt_dot[2] = QUAD_WAYPOINT_T_USE_DEFAULT;
  wmsg.xyzt_dot[3] = QUAD_WAYPOINT_T_USE_DEFAULT;
  wmsg.nonce = random();
  wmsg.utime = utime;

  if (x != 0)
    wmsg.xyzt[0] = x;
  if (y != 0)
    wmsg.xyzt[1] = y;
  if (z != 0)
    wmsg.xyzt[2] = z;
  if (yaw != 0)
    wmsg.xyzt[3] = yaw;
  wmsg.waypt_type = QUAD_WAYPOINT_T_TYPE_RELATIVE;
  quad_waypoint_t_publish(lcm, WAYPOINT_COMMAND_CHANNEL, &wmsg);
}
/*
 * carmen3d_controller_publish_body_relative_waypoint:
 * publishes a waypoint that instructs the quad to move relative to the BODY frame
 */
static inline void carmen3d_controller_publish_body_relative_waypoint(
    lcm_t * lcm, double x, double y, int64_t utime) {
  quad_waypoint_t wmsg;
  memset(&wmsg, 0, sizeof(wmsg));    //set everything to 0
  //  by previous/default unless otherwise instructed :-)
  wmsg.xyzt[0] = QUAD_WAYPOINT_T_USE_PREVIOUS;
  wmsg.xyzt[1] = QUAD_WAYPOINT_T_USE_PREVIOUS;
  wmsg.xyzt[2] = QUAD_WAYPOINT_T_USE_PREVIOUS;
  wmsg.xyzt[3] = QUAD_WAYPOINT_T_USE_PREVIOUS;
  wmsg.xyzt_dot[0] = QUAD_WAYPOINT_T_USE_DEFAULT;
  wmsg.xyzt_dot[1] = QUAD_WAYPOINT_T_USE_DEFAULT;
  wmsg.xyzt_dot[2] = QUAD_WAYPOINT_T_USE_DEFAULT;
  wmsg.xyzt_dot[3] = QUAD_WAYPOINT_T_USE_DEFAULT;
  wmsg.nonce = random();
  wmsg.utime = utime;

  if (x != 0)
    wmsg.xyzt[0] = x;
  if (y != 0)
    wmsg.xyzt[1] = y;
  wmsg.waypt_type = QUAD_WAYPOINT_T_TYPE_BODY_RELATIVE;
  quad_waypoint_t_publish(lcm, WAYPOINT_COMMAND_CHANNEL, &wmsg);
}

#ifdef __cplusplus
}
#endif

#endif
// @}
